(def font [
     0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ;   0
     0x00 0x00 0x00 0xF8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x33 0x30 0x00 0x00 0x00 ; ! 1
     0x00 0x10 0x0C 0x06 0x10 0x0C 0x06 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ; " 2
     0x40 0xC0 0x78 0x40 0xC0 0x78 0x40 0x00 0x04 0x3F 0x04 0x04 0x3F 0x04 0x04 0x00 ; # 3
     0x00 0x70 0x88 0xFC 0x08 0x30 0x00 0x00 0x00 0x18 0x20 0xFF 0x21 0x1E 0x00 0x00 ; $ 4
     0xF0 0x08 0xF0 0x00 0xE0 0x18 0x00 0x00 0x00 0x21 0x1C 0x03 0x1E 0x21 0x1E 0x00 ; % 5
     0x00 0xF0 0x08 0x88 0x70 0x00 0x00 0x00 0x1E 0x21 0x23 0x24 0x19 0x27 0x21 0x10 ; & 6
     0x10 0x16 0x0E 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ; ' 7
     0x00 0x00 0x00 0xE0 0x18 0x04 0x02 0x00 0x00 0x00 0x00 0x07 0x18 0x20 0x40 0x00 ; ( 8
     0x00 0x02 0x04 0x18 0xE0 0x00 0x00 0x00 0x00 0x40 0x20 0x18 0x07 0x00 0x00 0x00 ; ) 9
     0x40 0x40 0x80 0xF0 0x80 0x40 0x40 0x00 0x02 0x02 0x01 0x0F 0x01 0x02 0x02 0x00 ; * 10
     0x00 0x00 0x00 0xF0 0x00 0x00 0x00 0x00 0x01 0x01 0x01 0x1F 0x01 0x01 0x01 0x00 ; + 11
     0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x80 0xB0 0x70 0x00 0x00 0x00 0x00 0x00 ;  12
     0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x01 0x01 0x01 0x01 0x01 0x01 0x01 ; - 13
     0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x30 0x30 0x00 0x00 0x00 0x00 0x00 ; . 14
     0x00 0x00 0x00 0x00 0x80 0x60 0x18 0x04 0x00 0x60 0x18 0x06 0x01 0x00 0x00 0x00 ; / 15
     0x00 0xE0 0x10 0x08 0x08 0x10 0xE0 0x00 0x00 0x0F 0x10 0x20 0x20 0x10 0x0F 0x00 ; 0 16
     0x00 0x10 0x10 0xF8 0x00 0x00 0x00 0x00 0x00 0x20 0x20 0x3F 0x20 0x20 0x00 0x00 ; 1 17
     0x00 0x70 0x08 0x08 0x08 0x88 0x70 0x00 0x00 0x30 0x28 0x24 0x22 0x21 0x30 0x00 ; 2 18
     0x00 0x30 0x08 0x88 0x88 0x48 0x30 0x00 0x00 0x18 0x20 0x20 0x20 0x11 0x0E 0x00 ; 3 19
     0x00 0x00 0xC0 0x20 0x10 0xF8 0x00 0x00 0x00 0x07 0x04 0x24 0x24 0x3F 0x24 0x00 ; 4 20
     0x00 0xF8 0x08 0x88 0x88 0x08 0x08 0x00 0x00 0x19 0x21 0x20 0x20 0x11 0x0E 0x00 ; 5 21
     0x00 0xE0 0x10 0x88 0x88 0x18 0x00 0x00 0x00 0x0F 0x11 0x20 0x20 0x11 0x0E 0x00 ; 6 22
     0x00 0x38 0x08 0x08 0xC8 0x38 0x08 0x00 0x00 0x00 0x00 0x3F 0x00 0x00 0x00 0x00 ; 7 23
     0x00 0x70 0x88 0x08 0x08 0x88 0x70 0x00 0x00 0x1C 0x22 0x21 0x21 0x22 0x1C 0x00 ; 8 24
     0x00 0xE0 0x10 0x08 0x08 0x10 0xE0 0x00 0x00 0x00 0x31 0x22 0x22 0x11 0x0F 0x00 ; 9 25
     0x00 0x00 0x00 0xC0 0xC0 0x00 0x00 0x00 0x00 0x00 0x00 0x30 0x30 0x00 0x00 0x00 ; : 26
     0x00 0x00 0x00 0x80 0x00 0x00 0x00 0x00 0x00 0x00 0x80 0x60 0x00 0x00 0x00 0x00 ; ; 27
     0x00 0x00 0x80 0x40 0x20 0x10 0x08 0x00 0x00 0x01 0x02 0x04 0x08 0x10 0x20 0x00 ; < 28
     0x40 0x40 0x40 0x40 0x40 0x40 0x40 0x00 0x04 0x04 0x04 0x04 0x04 0x04 0x04 0x00 ; = 29
     0x00 0x08 0x10 0x20 0x40 0x80 0x00 0x00 0x00 0x20 0x10 0x08 0x04 0x02 0x01 0x00 ; > 30
     0x00 0x70 0x48 0x08 0x08 0x08 0xF0 0x00 0x00 0x00 0x00 0x30 0x36 0x01 0x00 0x00 ; ? 31
     0xC0 0x30 0xC8 0x28 0xE8 0x10 0xE0 0x00 0x07 0x18 0x27 0x24 0x23 0x14 0x0B 0x00 ; @ 32
     0x00 0x00 0xC0 0x38 0xE0 0x00 0x00 0x00 0x20 0x3C 0x23 0x02 0x02 0x27 0x38 0x20 ; A 33
     0x08 0xF8 0x88 0x88 0x88 0x70 0x00 0x00 0x20 0x3F 0x20 0x20 0x20 0x11 0x0E 0x00 ; B 34
     0xC0 0x30 0x08 0x08 0x08 0x08 0x38 0x00 0x07 0x18 0x20 0x20 0x20 0x10 0x08 0x00 ; C 35
     0x08 0xF8 0x08 0x08 0x08 0x10 0xE0 0x00 0x20 0x3F 0x20 0x20 0x20 0x10 0x0F 0x00 ; D 36
     0x08 0xF8 0x88 0x88 0xE8 0x08 0x10 0x00 0x20 0x3F 0x20 0x20 0x23 0x20 0x18 0x00 ; E 37
     0x08 0xF8 0x88 0x88 0xE8 0x08 0x10 0x00 0x20 0x3F 0x20 0x00 0x03 0x00 0x00 0x00 ; F 38
     0xC0 0x30 0x08 0x08 0x08 0x38 0x00 0x00 0x07 0x18 0x20 0x20 0x22 0x1E 0x02 0x00 ; G 39
     0x08 0xF8 0x08 0x00 0x00 0x08 0xF8 0x08 0x20 0x3F 0x21 0x01 0x01 0x21 0x3F 0x20 ; H 40
     0x00 0x08 0x08 0xF8 0x08 0x08 0x00 0x00 0x00 0x20 0x20 0x3F 0x20 0x20 0x00 0x00 ; I 41
     0x00 0x00 0x08 0x08 0xF8 0x08 0x08 0x00 0xC0 0x80 0x80 0x80 0x7F 0x00 0x00 0x00 ; J 42
     0x08 0xF8 0x88 0xC0 0x28 0x18 0x08 0x00 0x20 0x3F 0x20 0x01 0x26 0x38 0x20 0x00 ; K 43
     0x08 0xF8 0x08 0x00 0x00 0x00 0x00 0x00 0x20 0x3F 0x20 0x20 0x20 0x20 0x30 0x00 ; L 44
     0x08 0xF8 0xF8 0x00 0xF8 0xF8 0x08 0x00 0x20 0x3F 0x00 0x3F 0x00 0x3F 0x20 0x00 ; M 45
     0x08 0xF8 0x30 0xC0 0x00 0x08 0xF8 0x08 0x20 0x3F 0x20 0x00 0x07 0x18 0x3F 0x00 ; N 46
     0xE0 0x10 0x08 0x08 0x08 0x10 0xE0 0x00 0x0F 0x10 0x20 0x20 0x20 0x10 0x0F 0x00 ; O 47
     0x08 0xF8 0x08 0x08 0x08 0x08 0xF0 0x00 0x20 0x3F 0x21 0x01 0x01 0x01 0x00 0x00 ; P 48
     0xE0 0x10 0x08 0x08 0x08 0x10 0xE0 0x00 0x0F 0x18 0x24 0x24 0x38 0x50 0x4F 0x00 ; Q 49
     0x08 0xF8 0x88 0x88 0x88 0x88 0x70 0x00 0x20 0x3F 0x20 0x00 0x03 0x0C 0x30 0x20 ; R 50
     0x00 0x70 0x88 0x08 0x08 0x08 0x38 0x00 0x00 0x38 0x20 0x21 0x21 0x22 0x1C 0x00 ; S 51
     0x18 0x08 0x08 0xF8 0x08 0x08 0x18 0x00 0x00 0x00 0x20 0x3F 0x20 0x00 0x00 0x00 ; T 52
     0x08 0xF8 0x08 0x00 0x00 0x08 0xF8 0x08 0x00 0x1F 0x20 0x20 0x20 0x20 0x1F 0x00 ; U 53
     0x08 0x78 0x88 0x00 0x00 0xC8 0x38 0x08 0x00 0x00 0x07 0x38 0x0E 0x01 0x00 0x00 ; V 54
     0xF8 0x08 0x00 0xF8 0x00 0x08 0xF8 0x00 0x03 0x3C 0x07 0x00 0x07 0x3C 0x03 0x00 ; W 55
     0x08 0x18 0x68 0x80 0x80 0x68 0x18 0x08 0x20 0x30 0x2C 0x03 0x03 0x2C 0x30 0x20 ; X 56
     0x08 0x38 0xC8 0x00 0xC8 0x38 0x08 0x00 0x00 0x00 0x20 0x3F 0x20 0x00 0x00 0x00 ; Y 57
     0x10 0x08 0x08 0x08 0xC8 0x38 0x08 0x00 0x20 0x38 0x26 0x21 0x20 0x20 0x18 0x00 ; Z 58
     0x00 0x00 0x00 0xFE 0x02 0x02 0x02 0x00 0x00 0x00 0x00 0x7F 0x40 0x40 0x40 0x00 ; [ 59
     0x00 0x0C 0x30 0xC0 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x01 0x06 0x38 0xC0 0x00 ; \ 60
     0x00 0x02 0x02 0x02 0xFE 0x00 0x00 0x00 0x00 0x40 0x40 0x40 0x7F 0x00 0x00 0x00 ; ] 61
     0x00 0x00 0x04 0x02 0x02 0x02 0x04 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ; ^ 62
     0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x80 0x80 0x80 0x80 0x80 0x80 0x80 0x80 ; _ 63
     0x00 0x02 0x02 0x04 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ; ` 64
     0x00 0x00 0x80 0x80 0x80 0x80 0x00 0x00 0x00 0x19 0x24 0x22 0x22 0x22 0x3F 0x20 ; a 65
     0x08 0xF8 0x00 0x80 0x80 0x00 0x00 0x00 0x00 0x3F 0x11 0x20 0x20 0x11 0x0E 0x00 ; b 66
     0x00 0x00 0x00 0x80 0x80 0x80 0x00 0x00 0x00 0x0E 0x11 0x20 0x20 0x20 0x11 0x00 ; c 67
     0x00 0x00 0x00 0x80 0x80 0x88 0xF8 0x00 0x00 0x0E 0x11 0x20 0x20 0x10 0x3F 0x20 ; d 68
     0x00 0x00 0x80 0x80 0x80 0x80 0x00 0x00 0x00 0x1F 0x22 0x22 0x22 0x22 0x13 0x00 ; e 69
     0x00 0x80 0x80 0xF0 0x88 0x88 0x88 0x18 0x00 0x20 0x20 0x3F 0x20 0x20 0x00 0x00 ; f 70
     0x00 0x00 0x80 0x80 0x80 0x80 0x80 0x00 0x00 0x6B 0x94 0x94 0x94 0x93 0x60 0x00 ; g 71
     0x08 0xF8 0x00 0x80 0x80 0x80 0x00 0x00 0x20 0x3F 0x21 0x00 0x00 0x20 0x3F 0x20 ; h 72
     0x00 0x80 0x98 0x98 0x00 0x00 0x00 0x00 0x00 0x20 0x20 0x3F 0x20 0x20 0x00 0x00 ; i 73
     0x00 0x00 0x00 0x80 0x98 0x98 0x00 0x00 0x00 0xC0 0x80 0x80 0x80 0x7F 0x00 0x00 ; j 74
     0x08 0xF8 0x00 0x00 0x80 0x80 0x80 0x00 0x20 0x3F 0x24 0x02 0x2D 0x30 0x20 0x00 ; k 75
     0x00 0x08 0x08 0xF8 0x00 0x00 0x00 0x00 0x00 0x20 0x20 0x3F 0x20 0x20 0x00 0x00 ; l 76
     0x80 0x80 0x80 0x80 0x80 0x80 0x80 0x00 0x20 0x3F 0x20 0x00 0x3F 0x20 0x00 0x3F ; m 77
     0x80 0x80 0x00 0x80 0x80 0x80 0x00 0x00 0x20 0x3F 0x21 0x00 0x00 0x20 0x3F 0x20 ; n 78
     0x00 0x00 0x80 0x80 0x80 0x80 0x00 0x00 0x00 0x1F 0x20 0x20 0x20 0x20 0x1F 0x00 ; o 79
     0x80 0x80 0x00 0x80 0x80 0x00 0x00 0x00 0x80 0xFF 0xA1 0x20 0x20 0x11 0x0E 0x00 ; p 80
     0x00 0x00 0x00 0x80 0x80 0x80 0x80 0x00 0x00 0x0E 0x11 0x20 0x20 0xA0 0xFF 0x80 ; q 81
     0x80 0x80 0x80 0x00 0x80 0x80 0x80 0x00 0x20 0x20 0x3F 0x21 0x20 0x00 0x01 0x00 ; r 82
     0x00 0x00 0x80 0x80 0x80 0x80 0x80 0x00 0x00 0x33 0x24 0x24 0x24 0x24 0x19 0x00 ; s 83
     0x00 0x80 0x80 0xE0 0x80 0x80 0x00 0x00 0x00 0x00 0x00 0x1F 0x20 0x20 0x00 0x00 ; t 84
     0x80 0x80 0x00 0x00 0x00 0x80 0x80 0x00 0x00 0x1F 0x20 0x20 0x20 0x10 0x3F 0x20 ; u 85
     0x80 0x80 0x80 0x00 0x00 0x80 0x80 0x80 0x00 0x01 0x0E 0x30 0x08 0x06 0x01 0x00 ; v 86
     0x80 0x80 0x00 0x80 0x00 0x80 0x80 0x80 0x0F 0x30 0x0C 0x03 0x0C 0x30 0x0F 0x00 ; w 87
     0x00 0x80 0x80 0x00 0x80 0x80 0x80 0x00 0x00 0x20 0x31 0x2E 0x0E 0x31 0x20 0x00 ; x 88
     0x80 0x80 0x80 0x00 0x00 0x80 0x80 0x80 0x80 0x81 0x8E 0x70 0x18 0x06 0x01 0x00 ; y 89
     0x00 0x80 0x80 0x80 0x80 0x80 0x80 0x00 0x00 0x21 0x30 0x2C 0x22 0x21 0x30 0x00 ; z 90
     0x00 0x00 0x00 0x00 0x80 0x7C 0x02 0x02 0x00 0x00 0x00 0x00 0x00 0x3F 0x40 0x40 ; { 91
     0x00 0x00 0x00 0x00 0xFF 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0xFF 0x00 0x00 0x00 ; | 92
     0x00 0x02 0x02 0x7C 0x80 0x00 0x00 0x00 0x00 0x40 0x40 0x3F 0x00 0x00 0x00 0x00 ; } 93
     0x00 0x06 0x01 0x01 0x02 0x02 0x04 0x04 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 ; ~ 94
])

(defun putc (x row c) 
    (progn
        (bufcpy pixbuf (+ x 1 (* row 128)) font (* 16 (- c 32)) 8)
        (bufcpy pixbuf (+ x 1 (* (+ row 1) 128)) font (+ 8 (* 16 (- c 32))) 8)
))

(defun putstr (x row str)
    (looprange i 0 (str-len str)
        (putc (+ x (* i 8)) row (bufget-u8 str i))
))

(def #SSD-ADDRESS 0x3c)

(def cmds-init '(
    (0xAE)      ; Display off
    (0xD5 0x80) ; Osc freq
    (0xA8 0x3F) ; Mux ratio
    (0xD3 0x00) ; Display offset
    (0x8D 0x14) ; Char reg
    (0x81 0xCF) ; Set contrast
    (0x20 0x00) ; Memory addr mode
    (0x21 0 127) ; Column addr
    (0x22 0 7) ; Page addr
    (0x40) ; Start line
    (0xA1) ; Seg remap op
    (0xC8) ; Com scan dir op
    (0xDA 0x12) ; Com pin conf
    (0xD9 0xF1) ; Precharge
    (0xDB 0x40) ; Vcom deselect
    (0xA4) ; Dis ent disp on
    (0xA6) ; Dis normal
    (0x2E) ; Deactivate scroll
    (0xAF) ; Disaply on
))

(i2c-start 'rate-700k)

(loopforeach c cmds-init
    (i2c-tx-rx #SSD-ADDRESS `(0x00 ,@c))
)

(def pixbuf (array-create 1025))
(bufset-u8 pixbuf 0 0x40) ; First byte tells the SSD1306 that this is data
(bufclear pixbuf 0 1)

; Note that the functions below assume int for x, y and row, so a type conversion
; using (to-i x) has to be made if they are called with e.g. floats.

(defun pix-set (x y)
    (bufset-bit pixbuf (+ 8 (* x 8) (mod y 8) (* (/ y 8) 1024)) 1)
)

(defun circle (x0 y0 r)
    (let (
        (f (- 1 r))
        (ddF-x 1)
        (ddF-y (* -2 r))
        (x 0)
        (y r)
    )
    (progn
        (pix-set x0 (+ y0 r))
        (pix-set x0 (- y0 r))
        (pix-set (+ x0 r) y0)
        (pix-set (- x0 r) y0)

        (loopwhile (< x y)
            (progn
                (if (>= f 0)
                    (progn
                        (setvar 'y (- y 1))
                        (setvar 'ddF-y (+ ddF-y 2))
                        (setvar 'f (+ f ddF-y))
                ))
                (setvar 'x (+ x 1))
                (setvar 'ddF-x (+ ddF-x 2))
                (setvar 'f (+ f ddF-x))
                
                (pix-set (+ x0 x) (+ y0 y))
                (pix-set (- x0 x) (+ y0 y))
                (pix-set (+ x0 x) (- y0 y))
                (pix-set (- x0 x) (- y0 y))
                (pix-set (+ x0 y) (+ y0 x))
                (pix-set (- x0 y) (+ y0 x))
                (pix-set (+ x0 y) (- y0 x))
                (pix-set (- x0 y) (- y0 x))
)))))

(defun line (x0 y0 x1 y1)
    (let (
        (dx (abs (- x1 x0)))
        (sx (if (< x0 x1) 1 -1))
        (dy (- (abs (- y1 y0))))
        (sy (if (< y0 y1) 1 -1))
        (error (+ dx dy))
    )
    (loopwhile t
        (progn
            (pix-set x0 y0)
            (if (and (= x0 x1) (= y0 y1)) (break))
            (if (>= (* 2 error) dy)
                (progn
                    (if (= x0 x1) (break))
                    (setvar 'error (+ error dy))
                    (setvar 'x0 (+ x0 sx))))
            (if (<= (* 2 error) dx)
                (progn
                    (if (= y0 y1) (break))
                    (setvar 'error (+ error dx))
                    (setvar 'y0 (+ y0 sy))))
))))

(def fps 0)

(loopwhile t
    (progn
        (def t-start (systime))
        (putstr 0 0 (str-from-n (get-vin)      "V In : %.1f "))
        (putstr 0 2 (str-from-n (get-temp-fet) "T Fet: %.1f "))
        (putstr 0 6 (str-from-n fps "FPS %.1f "))
        (circle 90 50 (to-i (- (get-vin) 10))) ; Circle that changes size based in v_in
        (i2c-tx-rx #SSD-ADDRESS pixbuf)
        (bufclear pixbuf 0 1)
        (def fps (/ 1 (secs-since t-start))) ; Calculate framerate excluding sleep
        (sleep 0.1)
))
